#include "osal_msg.h"
#include "osal_memory.h"
#include "osal_event.h"

#define SYS_EVENT_MSG               0x8000

/*********************************************************************
 * @fn osal_msg_allocate
 *
 * @brief
 *
 *    This function is called by a task to allocate a message buffer
 *    into which the task will encode the particular message it wishes
 *    to send.  This common buffer scheme is used to strictly limit the
 *    creation of message buffers within the system due to RAM size
 *    limitations on the microprocessor.   Note that all message buffers
 *    are a fixed size (at least initially).  The parameter len is kept
 *    in case a message pool with varying fixed message sizes is later
 *    created (for example, a pool of message buffers of size LARGE,
 *    MEDIUM and SMALL could be maintained and allocated based on request
 *    from the tasks).
 *
 * @param   uint16 len  - wanted buffer length
 * @return  pointer to allocated buffer or NULL if allocation failed.
 */
uint8* osal_msg_allocate (uint16 len)
{
    osal_msg_hdr_t* hdr;

    if (len == 0)
    {
        return (NULL);
    }

    hdr = (osal_msg_hdr_t*) osal_mem_alloc ( (short) (len + sizeof (osal_msg_hdr_t) ) );
    if (hdr)
    {
        hdr->next = NULL;
        hdr->len = len;
        hdr->dest_id = TASK_NO_TASK;
        return ( (uint8*) (hdr + 1) );
    }
    else
    {
        return (NULL);
    }
}

/*********************************************************************
 * @fn osal_msg_deallocate
 *
 * @brief
 *
 *    This function is used to deallocate a message buffer. This function
 *    is called by a task (or processing element) after it has finished
 *    processing a received message.
 *
 *
 * @param   uint8 *msg_ptr - pointer to new message buffer
 *
 * @return  SUCCESS, INVALID_MSG_POINTER
 */
uint8 osal_msg_deallocate (uint8* msg_ptr)
{
    uint8* x;

    if (msg_ptr == NULL)
    {
        return (INVALID_MSG_POINTER);
    }

    // don't deallocate queued buffer
    if (OSAL_MSG_ID (msg_ptr) != TASK_NO_TASK)
    {
        return (MSG_BUFFER_NOT_AVAIL);
    }

    x = (uint8*) ( (uint8*) msg_ptr - sizeof (osal_msg_hdr_t) );

    osal_mem_free ( (void*) x);

    return (SUCCESS);
}

/*********************************************************************
 * @fn osal_msg_send
 *
 * @brief
 *
 *    This function is called by a task to send a command message to
 *    another task or processing element.  The sending_task field must
 *    refer to a valid task, since the task ID will be used
 *    for the response message.  This function will also set a message
 *    ready event in the destination tasks event list.
 *
 *
 * @param   uint8 destination task - Send msg to?  Task ID
 * @param   uint8 *msg_ptr - pointer to new message buffer
 *
 * @return  SUCCESS, INVALID_TASK, INVALID_MSG_POINTER
 */
uint8 osal_msg_send (uint8 destination_task, uint8* msg_ptr)
{
	osal_msg_hdr_t *msg_ptr_tmp = NULL;
    if (msg_ptr == NULL)
    {
        return (INVALID_MSG_POINTER);
    }

    if (destination_task >= tasksCnt)
    {
        osal_msg_deallocate (msg_ptr);
        return (INVALID_TASK);
    }

	msg_ptr_tmp = (osal_msg_hdr_t *)(msg_ptr);
    // Check the message header
    if (OSAL_MSG_NEXT (msg_ptr) != NULL ||
            OSAL_MSG_ID (msg_ptr) != TASK_NO_TASK)
    {
        osal_msg_deallocate (msg_ptr);
        return (INVALID_MSG_POINTER);
    }

    OSAL_MSG_ID (msg_ptr) = destination_task;

    // queue message
    osal_msg_enqueue (&osal_qHead, msg_ptr);

    // Signal the task that a message is waiting
    osal_set_event (destination_task, SYS_EVENT_MSG);

    return (SUCCESS);
}

/*********************************************************************
 * @fn osal_msg_receive
 *
 * @brief
 *
 *    This function is called by a task to retrieve a received command
 *    message. The calling task must deallocate the message buffer after
 *    processing the message using the osal_msg_deallocate() call.
 *
 * @param   uint8 task_id - receiving tasks ID
 *
 * @return  *uint8 - message information or NULL if no message
 */
uint8* osal_msg_receive (uint8 task_id)
{
    osal_msg_hdr_t* listHdr;
    osal_msg_hdr_t* prevHdr = NULL;
    osal_msg_hdr_t* foundHdr = NULL;


    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    // Point to the top of the queue
    listHdr = osal_qHead;

    // Look through the queue for a message that belongs to the asking task
    while (listHdr != NULL)
    {
        if ( (listHdr - 1)->dest_id == task_id)
        {
            if (foundHdr == NULL)
            {
                // Save the first one
                foundHdr = listHdr;
            }
            else
            {
                // Second msg found, stop looking
                break;
            }
        }
        if (foundHdr == NULL)
        {
            prevHdr = listHdr;
        }
        listHdr = OSAL_MSG_NEXT (listHdr);
    }

    // Is there more than one?
    if (listHdr != NULL)
    {
        // Yes, Signal the task that a message is waiting
        osal_set_event (task_id, SYS_EVENT_MSG);
    }
    else
    {
        // No more
        osal_clear_event (task_id, SYS_EVENT_MSG);
    }

    // Did we find a message?
    if (foundHdr != NULL)
    {
        // Take out of the link list
        osal_msg_extract (&osal_qHead, foundHdr, prevHdr);
    }

    // Release interrupts
    HAL_EXIT_CRITICAL_SECTION();

    return ( (uint8*) foundHdr);
}

/**************************************************************************************************
 * @fn     osal_msg_find
 *
 * @brief       This function finds in place an OSAL message matching the task_id and event
 *              parameters.
 *
 * input parameters
 *
 * @param       task_id - The OSAL task id that the enqueued OSAL message must match.
 * @param       event - The OSAL event id that the enqueued OSAL message must match.
 *
 * output parameters
 *
 * None.
 *
 * @return      NULL if no match, otherwise an in place pointer to the matching OSAL message.
 **************************************************************************************************
 */
osal_event_hdr_t* osal_msg_find (uint8 task_id, uint8 event)
{
    osal_msg_hdr_t* pHdr;


    HAL_ENTER_CRITICAL_SECTION();  // Hold off interrupts.

    pHdr = osal_qHead;  // Point to the top of the queue.

    // Look through the queue for a message that matches the task_id and event parameters.
    while (pHdr != NULL)
    {
        if ( ( (pHdr - 1)->dest_id == task_id) && ( ( (osal_event_hdr_t*) pHdr)->event == event) )
        {
            break;
        }

        pHdr = OSAL_MSG_NEXT (pHdr);
    }

    HAL_EXIT_CRITICAL_SECTION();  // Release interrupts.

    return (osal_event_hdr_t*) pHdr;
}

/*********************************************************************
 * @fn osal_msg_enqueue
 *
 * @brief
 *
 *    This function enqueues an OSAL message into an OSAL queue.
 *
 * @param   osal_msg_q_t *q_ptr - OSAL queue
 * @param   void *msg_ptr  - OSAL message
 *
 * @return  none
 */
void osal_msg_enqueue (osal_msg_q_t* q_ptr, void* msg_ptr)
{
    void* list;


    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    OSAL_MSG_NEXT (msg_ptr) = NULL;
    // If first message in queue
    if (*q_ptr == NULL)
    {
        *q_ptr = msg_ptr;
    }
    else
    {
        // Find end of queue
        for (list = *q_ptr; OSAL_MSG_NEXT (list) != NULL; list = OSAL_MSG_NEXT (list) );

        // Add message to end of queue
        OSAL_MSG_NEXT (list) = msg_ptr;
    }

    // Re-enable interrupts
    HAL_EXIT_CRITICAL_SECTION();
}

/*********************************************************************
 * @fn osal_msg_dequeue
 *
 * @brief
 *
 *    This function dequeues an OSAL message from an OSAL queue.
 *
 * @param   osal_msg_q_t *q_ptr - OSAL queue
 *
 * @return  void * - pointer to OSAL message or NULL of queue is empty.
 */
void* osal_msg_dequeue (osal_msg_q_t* q_ptr)
{
    void* msg_ptr = NULL;


    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    if (*q_ptr != NULL)
    {
        // Dequeue message
        msg_ptr = *q_ptr;
        *q_ptr = OSAL_MSG_NEXT (msg_ptr);
        OSAL_MSG_NEXT (msg_ptr) = NULL;
        OSAL_MSG_ID (msg_ptr) = TASK_NO_TASK;
    }

    // Re-enable interrupts
    HAL_EXIT_CRITICAL_SECTION();

    return msg_ptr;
}

/*********************************************************************
 * @fn osal_msg_push
 *
 * @brief
 *
 *    This function pushes an OSAL message to the head of an OSAL
 *    queue.
 *
 * @param   osal_msg_q_t *q_ptr - OSAL queue
 * @param   void *msg_ptr  - OSAL message
 *
 * @return  none
 */
void osal_msg_push (osal_msg_q_t* q_ptr, void* msg_ptr)
{
    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    // Push message to head of queue
    OSAL_MSG_NEXT (msg_ptr) = *q_ptr;
    *q_ptr = msg_ptr;

    // Re-enable interrupts
    HAL_EXIT_CRITICAL_SECTION();
}

/*********************************************************************
 * @fn osal_msg_extract
 *
 * @brief
 *
 *    This function extracts and removes an OSAL message from the
 *    middle of an OSAL queue.
 *
 * @param   osal_msg_q_t *q_ptr - OSAL queue
 * @param   void *msg_ptr  - OSAL message to be extracted
 * @param   void *prev_ptr  - OSAL message before msg_ptr in queue
 *
 * @return  none
 */
void osal_msg_extract (osal_msg_q_t* q_ptr, void* msg_ptr, void* prev_ptr)
{
    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    if (msg_ptr == *q_ptr)
    {
        // remove from first
        *q_ptr = OSAL_MSG_NEXT (msg_ptr);
    }
    else
    {
        // remove from middle
        OSAL_MSG_NEXT (prev_ptr) = OSAL_MSG_NEXT (msg_ptr);
    }
    OSAL_MSG_NEXT (msg_ptr) = NULL;
    OSAL_MSG_ID (msg_ptr) = TASK_NO_TASK;

    // Re-enable interrupts
    HAL_EXIT_CRITICAL_SECTION();
}

/*********************************************************************
 * @fn osal_msg_enqueue_max
 *
 * @brief
 *
 *    This function enqueues an OSAL message into an OSAL queue if
 *    the length of the queue is less than max.
 *
 * @param   osal_msg_q_t *q_ptr - OSAL queue
 * @param   void *msg_ptr  - OSAL message
 * @param   uint8 max - maximum length of queue
 *
 * @return  TRUE if message was enqueued, FALSE otherwise
 */
uint8 osal_msg_enqueue_max (osal_msg_q_t* q_ptr, void* msg_ptr, uint8 max)
{
    void* list;
    uint8 ret = FALSE;

    // Hold off interrupts
    HAL_ENTER_CRITICAL_SECTION();

    // If first message in queue
    if (*q_ptr == NULL)
    {
        *q_ptr = msg_ptr;
        ret = TRUE;
    }
    else
    {
        // Find end of queue or max
        list = *q_ptr;
        max--;
        while ( (OSAL_MSG_NEXT (list) != NULL) && (max > 0) )
        {
            list = OSAL_MSG_NEXT (list);
            max--;
        }

        // Add message to end of queue if max not reached
        if (max != 0)
        {
            OSAL_MSG_NEXT (list) = msg_ptr;
            ret = TRUE;
        }
    }

    // Re-enable interrupts
    HAL_EXIT_CRITICAL_SECTION();

    return ret;
}
